//
// Created by 16933 on 2022/5/2.
//

#include "Pid.h"
#include <stdint.h>
static void abs_limit(float *a, float ABS_MAX)
{
    if (*a > ABS_MAX)
        *a = ABS_MAX;
    if (*a < -ABS_MAX)
        *a = -ABS_MAX;
}
/**
  * @brief     PID 初始化函数
  * @param[in] pid: PID控制器指针
  * @param[in] max_out: 最大输出限幅
  * @param[in] intergral_limit: 积分限幅
  * @param[in] kp/ki/kd: 具体 PID 参数
  * @param[in] death_space: 死区
  */
void pid_init(pid_t *pid, uint32_t max_out, uint32_t intergral_limit,float kp, float ki, float kd,uint32_t death_space)
{
    pid->integral_limit = intergral_limit;
    pid->max_output     = max_out;

    pid->p = kp;
    pid->i = ki;
    pid->d = kd;
    pid->death_space = death_space;
}


/**
  * @brief     PID 计算函数，使用位置式 PID 计算
  * @param[in] pid: PID 结构体
  * @param[in] get: 反馈数据
  * @param[in] set: 目标数据
  * @retval    PID 计算输出
  */
float pid_calc(pid_t *pid, float get, float set)
{
    pid->get = get;
    pid->set = set;
    pid->err[NOW] = set - get;

    pid->pout = pid->p * pid->err[NOW];
    pid->iout += pid->i * pid->err[NOW];
    pid->dout = pid->d * (pid->err[NOW] - pid->err[LAST]);

    abs_limit(&(pid->iout), pid->integral_limit);
    if (set==0)
    {
        pid->out = pid->pout + pid->iout + pid->dout;
    }
    else{
        pid->out = pid->pout + pid->iout + pid->dout + pid->death_space;
    }

    abs_limit(&(pid->out), pid->max_output);

    pid->err[LAST]  = pid->err[NOW];

    return pid->out;
}


/**
  * @brief     PID 参数复位函数
  * @param[in] pid: PID 结构体
  * @param[in] kp/ki/kd: 具体 PID 参数
  */
void pid_reset(pid_t *pid, float kp, float ki, float kd)
{
    pid->p = kp;
    pid->i = ki;
    pid->d = kd;

    pid->pout = 0;
    pid->iout = 0;
    pid->dout = 0;
    pid->out  = 0;
}

float pid_calc_ang(pid_t *pid, float get, float set,short gy)
{
    pid->get = get;
    pid->set = set;
    pid->err[NOW] = set - get;

    pid->pout = pid->p * pid->err[NOW];
    pid->iout += pid->i * pid->err[NOW];
    pid->dout = pid->d * gy;

    abs_limit(&(pid->iout), pid->integral_limit);
    if (set==0)
    {
        pid->out = pid->pout + pid->iout + pid->dout;
    }
    else{
        pid->out = pid->pout + pid->iout + pid->dout + pid->death_space;
    }

    abs_limit(&(pid->out), pid->max_output);

    pid->err[LAST]  = pid->err[NOW];

    return pid->out;
}